#include "slep.h"



double* oneVector;
double Inf = DBL_MAX;

void initSLEP(int maxDimension){
		int i=0;
		oneVector = (double*) malloc( sizeof(double)* maxDimension );
		for(i=0; i < maxDimension ; i++)
			oneVector[i] = 1.0;
}

void initOPTS(OPTS* opt,int m,int n,int k){
	int i=0;

	opt->m = m;
	opt->n = n;

	opt->registry[0] = '\0';
	opt->init = 2;
	opt->x0 = NULL;
	opt->c0 = 0.0;

	//Termination
	opt->maxIter = 10000;
	opt->tol = 0.0001;
	opt->tFlag = 5; // Max iterations is default

	//Normalization
	opt->nFlag = 0;
	opt->mu = NULL;
	opt->nu = NULL;

	// Method
	opt->method = 0;
	opt->lFlag = 0;

	// Regularization
	opt->rFlag = 0;
	opt->rsL2 = 0.0;

}

void setOPTx0(OPTS* opt,double* x0){
	if(strstr(opt->registry,"x0")== NULL && x0!= NULL)
		strcat(opt->registry,"x0");
	opt->x0 = x0;
}

void setOPTc0(OPTS* opt,double c0){
	if(strstr(opt->registry,"c0")== NULL )
		strcat(opt->registry,"c0");
	opt->c0 = c0;
}
void setOPTmt_c0(OPTS *opt,double* mt_c0){
	if(strstr(opt->registry,"mt_c0")== NULL && mt_c0!= NULL)
		strcat(opt->registry,"mt_c0");
	opt->mt_c0 = mt_c0;
}

void setOPTmc_c0(OPTS *opt,double* mc_c0){
	if(strstr(opt->registry,"mc_c0")== NULL && mc_c0!= NULL)
		strcat(opt->registry,"mc_c0");
	opt->mc_c0 = mc_c0;
}


void setOPTmu(OPTS* opt,double* mu){
	if(strstr(opt->registry,"mu")== NULL && mu!= NULL)
		strcat(opt->registry,"mu");
	opt->mu = mu;
}

void setOPTnu(OPTS* opt,double* nu){
	if(strstr(opt->registry,"nu")== NULL && nu!= NULL)
		strcat(opt->registry,"nu");
	opt->nu = nu;
}

void setOPTsWeight(OPTS* opt,double* sWeight){
	if(strstr(opt->registry,"sWeight")== NULL && sWeight!= NULL)
		strcat(opt->registry,"sWeight");
	opt->sWeight[0] = sWeight[0];
	opt->sWeight[1] = sWeight[1];
}



void setOPTind(OPTS* opt,int* ind, int ind_size){
	if(strstr(opt->registry,"ind")== NULL && ind!= NULL)
		strcat(opt->registry,"ind");
	opt->ind = ind;
	opt->ind_size = ind_size;
}

void setOPTq(OPTS* opt,double q){
	if(strstr(opt->registry,"q")== NULL && q>1.0)
		strcat(opt->registry,"q");
	opt->q = q;
}

void setOPTgWeights(OPTS* opt,double *gWeights){
	if(strstr(opt->registry,"gWeights")== NULL && gWeights!= NULL)
		strcat(opt->registry,"gWeights");
	opt->gWeights = gWeights;
}

int hasGot(OPTS *opt,char * field){
	if(strstr(opt->registry,field)== NULL)
		return 0;
	return 1;
}

int terminationCondition(OPTS* opt, double fVal,double fValp, double* x, double norm_xxp,double norm_xp,int curIter){
	int bFlag =0;
	    // switch(opts.tFlag)
	switch(opt->tFlag){
        //    case 0
        //        if iterStep>=2
        //            if (abs( funVal(iterStep) - funVal(iterStep-1) ) <= opts.tol)
        //                break;
        //            end
        //        end
		case 0:
			if(curIter >= 2)
				if( fabs(fVal-fValp) <= opt->tol)
					bFlag = 1;
			break;

        //    case 1
        //        if iterStep>=2
        //            if (abs( funVal(iterStep) - funVal(iterStep-1) ) <=...
        //                    opts.tol* funVal(iterStep-1))
        //                break;
        //            end
        //        end
		case 1:
			if( curIter >=2)
				if(  fabs(fVal-fValp) <= (opt->tol * fValp) )
					bFlag = 1;
			break;

        //    case 2
        //        if ( funVal(iterStep)<= opts.tol)
        //            break;
        //        end
		case 2:
			if ( fVal <= opt->tol)
				bFlag = 1;
			break;

        //		case 3
        //			norm_xxp=sqrt(xxp'*xxp);
		//			if ( norm_xxp <=opts.tol)
        //				break;

        //    case 3
        //        norm_xxp=sqrt(xxp'*xxp + norm(t-tp)^2);
        //        if ( norm_xxp <=opts.tol)
        //            break;
        //        end
		case 3:
			if ( norm_xxp <= opt->tol)
				bFlag = 1;
			break;

            //case 4
            //    norm_xp=sqrt(xp'*xp);    norm_xxp=sqrt(xxp'*xxp);
            //    if ( norm_xxp <=opts.tol * max(norm_xp,1))
            //        break;
            //    end

        //    case 4
        //        norm_xp=sqrt(xp'*xp + tp'*tp);    norm_xxp=sqrt(xxp'*xxp+ norm(t-tp)^2);
        //        if ( norm_xxp <=opts.tol * max(norm_xp,1))
        //            break;
        //        end
		case 4:
			if ( norm_xxp <= opt->tol * maxof2(norm_xp,1.0) )
				bFlag = 1;
			break;


        //    case 5
        //        if iterStep>=opts.maxIter
        //            break;
        //        end
		case 5:
			if( curIter >= opt->maxIter)
				bFlag = 1;
			break;
        //end
	}
	return bFlag;

}



void initNormalization(OPTS *opt,double *A){

	//%%%%%initialization of one vector can be moved elsewhere
	int maxval = maxof2(opt->m,opt->n),i,j;
	double norm2Val;
	if(opt->nFlag !=0){
		if(!hasGot(opt,"mu")){
			opt->mu = (double *) malloc(sizeof(double)* opt->n);

			dgemv('T',opt->m,opt->n, (1.0/((double)opt->m)), A,opt->m,oneVector,1,0.0,opt->mu,1);
		}
		if(opt->nFlag ==1){
			if(!hasGot(opt,"nu")){
				opt->nu = (double *) malloc(sizeof(double)* opt->n);
				for(i=0;i<opt->n;i++){
					norm2Val = dnrm2(opt->m,(A+i*opt->m),1);
					(opt->nu)[i] = sqrt( norm2Val * norm2Val  / ((double)opt->m));
				}
				if( (opt->nu)[i] < 1.0e-10 )
					(opt->nu)[i] = 1.0;
			}
		}
		else
			if(!hasGot(opt,"nu")){
				opt->nu = (double *) malloc(sizeof(double)* opt->m);
				for(i=0;i<opt->m;i++){
					(opt->nu)[i]=0.0;
					for(j=0;j<opt->n;j++)
						(opt->nu)[i] += A[i +j* opt->m]*A[i +j* opt->m];
					(opt->nu)[i] = sqrt((opt->nu)[i]/ ((double)opt->n) );
					if( (opt->nu)[i] < 1.0e-10 )
						(opt->nu)[i] = 1.0;
				}
			}
	}
}


void normalizedmv(char trans,OPTS* opt,double* A, double* x, double* y){
	int i;
	double sumx;
	double * invNu;
	if(opt->nFlag == 0){
		dgemv(trans,opt->m,opt->n,1.0,A,opt->m,x,1,0.0,y,1);
		return;
	}
//elseif (opts.nFlag==1)
//    ATy=A'*y - sum(y) * mu';  ATy=ATy./nu;
//else
//    invNu=y./nu;              ATy=A'*invNu-sum(invNu)*mu';
//end
	
		if(trans == 'T' || trans == 't'){
			if(opt->nFlag ==1){
				sumx = ddot(opt->m,x,1,oneVector,1);
				dcopy(opt->n,opt->mu,1,y,1);
				dgemv(trans,opt->m,opt->n,1.0,A,opt->m,x,1,(0.0-sumx),y,1);
				for(i = 0;i<opt->n; i++)
					y[i] = y[i] / (opt->nu)[i];
			}
			else{
				invNu = (double*) malloc( sizeof(double)* opt->m );
				for(i = 0;i<opt->m; i++)
					invNu[i] = x[i] / (opt->nu)[i];
				sumx =  ddot(opt->m,invNu,1,oneVector,1);
				dcopy(opt->n,opt->mu,1,y,1);
				dgemv(trans,opt->m,opt->n,1.0,A,opt->m,invNu,1,(0.0-sumx),y,1);
			}
		}
//elseif (opts.nFlag==1)
//    invNu=x./nu; mu_invNu=mu * invNu;
//    Ax=A*invNu -repmat(mu_invNu, m, 1);
//else
//    Ax=A*x-repmat(mu*x, m, 1);     Ax=Ax./nu;
//end
		else{
			if(opt->nFlag ==1){
				invNu = (double*) malloc( sizeof(double)* opt->n );
				for(i = 0;i<opt->n; i++)
					invNu[i] = x[i] / (opt->nu)[i];
				sumx =  ddot(opt->n,invNu,1,opt->mu,1);
				dcopy(opt->m,oneVector,1,y,1);
				dgemv(trans,opt->m,opt->n,1.0,A,opt->m,invNu,1,(0.0-sumx),y,1);
			}
			else{
				sumx = ddot(opt->n,x,1,opt->mu,1);
				dcopy(opt->n,oneVector,1,y,1);
				dgemv(trans,opt->m,opt->n,1.0,A,opt->m,x,1,(0.0-sumx),y,1);
			}
		}
}





void errorOut(char* errorMsg){
	printf("\nThe program terminated as there was an error.\n");
	printf("%s\n",errorMsg);
	printf("Please correct the error and try again. Thank you.\n");
	exit(1);
}



double dnrm(int n,double* x,int incx, double nrmbase){
	int i = 0;
	double sum=0.0;
	if( nrmbase == DBL_MAX)
		return idamax(n,x,incx);
	if( nrmbase>0.999 && nrmbase<1.0001)
		return dasum(n,x,incx);
	if(nrmbase > 1.999 && nrmbase < 2.0001)
		return dnrm2(n,x,incx);
	for ( i = 0 ; i < n ; i++ )
		sum += pow(x[i],nrmbase);
	return pow(sum,1.0/nrmbase);
}



void normalizedmv_lowlevel(char trans,int m,int n,double* A, int lda,double* x, double* y, int mstart, int nstart, OPTS *opt){
	//int i;
	double sumx;
	double * invNu;
	if(trans == 'T' || trans == 't')
		memset(y,0,sizeof(double)*n);
	else
		memset(y,0,sizeof(double)*m);
	if(opt->nFlag == 0){
		dgemv(trans,m,n,1.0,A,lda,x,1,0.0,y,1);
		return;
	}
//elseif (opts.nFlag==1)
//    ATy=A'*y - sum(y) * mu';  ATy=ATy./nu;
//else
//    invNu=y./nu;              ATy=A'*invNu-sum(invNu)*mu';
//end
	
		if(trans == 'T' || trans == 't'){
			if(opt->nFlag ==1){
				sumx = ddot(m,x,1,oneVector,1);
				dcopy(n,&(opt->mu)[nstart],1,y,1);
				dgemv(trans,m,n,1.0,A,lda,x,1,(0.0-sumx),y,1);

				//for(i = 0;i<opt->n; i++)
				//	y[i] = y[i] / (opt->nu)[i];
				dtbsv('U','N','N',n,0,&(opt->nu)[nstart],1,y,1);
			}
			else{
				invNu = (double*) malloc( sizeof(double)* m );

//				for(i = 0;i< m; i++)
//					invNu[i] = x[i] / (opt->nu)[i];
				dcopy(m,x,1,invNu,1);
				dtbsv('U','N','N',n,0,&(opt->nu)[mstart],1,invNu,1);

				sumx =  ddot(m,invNu,1,oneVector,1);
				dcopy(n,&(opt->mu)[nstart],1,y,1);
				dgemv(trans,m,n,1.0,A,lda,invNu,1,(0.0-sumx),y,1);
			}
		}
//elseif (opts.nFlag==1)
//    invNu=x./nu; mu_invNu=mu * invNu;
//    Ax=A*invNu -repmat(mu_invNu, m, 1);
//else
//    Ax=A*x-repmat(mu*x, m, 1);     Ax=Ax./nu;
//end
		else{
			if(opt->nFlag ==1){
				invNu = (double*) malloc( sizeof(double)* n );
//				for(i = 0;i< n; i++)
//					invNu[i] = x[i] / (opt->nu)[i];
				dcopy(n,x,1,invNu,1);
				dtbsv('U','N','N',n,0,&(opt->nu)[nstart],1,invNu,1);
				sumx =  ddot(n,invNu,1,opt->mu,1);
				dcopy(m,oneVector,1,y,1);
				dgemv(trans,m,n,1.0,A,lda,invNu,1,(0.0-sumx),y,1);
			}
			else{
				sumx = ddot(n,x,1,&(opt->mu)[nstart],1);
				dcopy(n,oneVector,1,y,1);
				dgemv(trans,m,n,1.0,A,lda,x,1,(0.0-sumx),y,1);
			}
		}
}


void normalizedMM(char trans, int m, int n, int k, double* A, double* x, double* y, OPTS* opt){
/*	% compute AT y
if (opts.nFlag==0)
    ATy =A'* y;
elseif (opts.nFlag==1)
    ATy= A'* y -  mu' * sum(y, 1);    ATy=ATy./repmat(nu, 1, k);
else
    invNu=y./repmat(nu, 1, k);        ATy=A'*invNu-mu' * sum(invNu, 1);
end*/
	double *tmp1k, *invNu;
	int i;
	tmp1k = (double *) malloc(sizeof(double)*k);
	invNu = (double *) malloc(sizeof(double)*k*maxof2(m,n));

	if(trans== 'T' || trans=='t'){
		if(opt->nFlag == 0)
			dgemm(trans,'N',n,k,m,1.0,A,m,x,m,0.0,y,n);
		else if( opt->nFlag == 1){
			dgemv('T',m,k,1.0,y,m,oneVector,1,0.0,tmp1k,1);
			dgemm(trans,'N',n,k,m,1.0,A,m,x,m,0.0,y,n);
			dger(n,k,-1.0,opt->mu,1,tmp1k,1,y,n);
			for(i = 0; i<k ; i ++ )
				dtbsv('U','N','N',n,0,opt->nu,1,&y[i*n],1);
		}
		else{
			dcopy(m*k,x,1,invNu,1);
			for(i = 0; i<k ; i ++ )
				dtbsv('U','N','N',m,0,opt->nu,1,&invNu[i*m],1);
			dgemm('T','N',n,k,m,1.0,A,m,invNu,m,0.0,y,n);
			dgemv('T',m,k,1.0,invNu,m,oneVector,1,0.0,tmp1k,1);
			dger(n,k,-1.0,opt->mu,1,tmp1k,1,y,n);
		}
	}
	// TO-DO incomplete has to have methods to suport where trans=='N'
	// completed - ignore the above todo

	/*
if (opts.nFlag==0)
    Ax=A* x;
elseif (opts.nFlag==1)
    invNu=x./repmat(nu, 1, k);  mu_invNu=mu * invNu;
    Ax=A*invNu -repmat(mu_invNu, m, 1);
else
    Ax=A*x-repmat(mu*x, m, 1);     Ax=Ax./repmat(nu, 1, k);
end*/
	else{
		if(opt->nFlag == 0)
			dgemm(trans,'N',m,k,n,1.0,A,m,x,n,0.0,y,m);
		else if( opt->nFlag == 1){
			dcopy(n*k,x,1,invNu,1);
			for(i=0;i<k;i++)
				dtbsv('U','N','N',n,0,opt->nu,1,&invNu[i*n],1);
			dgemv('T',n,k,1.0,invNu,n,opt->mu,1,0.0,tmp1k,1);
			dgemv('N',m,n,1.0,A,m,invNu,1,0.0,y,1);
			dger(m,k,-1.0,oneVector,1,tmp1k,1,y,m);
		}
		else{
			dgemv('T',n,k,1.0,x,n,opt->mu,1,0.0,tmp1k,1);
			dgemm(trans,'N',m,k,n,1.0,A,m,x,n,0.0,y,m);
			dger(m,k,-1.0,oneVector,1,tmp1k,1,y,m);
			for(i=0;i<k;i++)
				dtbsv('U','N','N',n,0,opt->nu,1,&y[i*n],1);
		}
	}
	free(tmp1k);
	free(invNu);

}